#include "command.h"
#include "math.h"
#include <stdio.h>
#include "behavior.h"
#include "serial.h"
/*
f cm (cm/s)
b cm (cm/s)

l deg
r deg
*/


static char prefix;
static int param;
static int i = 0;
static char buffer[16];
static int zero_received = 0;

typedef enum {
	FREE,
	START,
	PARAM,
	HANDLING,
} ParseState;


static ParseState state;
inline int is_prefix(char c) {
	return c == 'f' || c == 'b' || c == 'r' || c == 'l';
}
inline int is_num(char c) {
	return ('0'<=c && c<='9') || c == '-';
}
int handle_command(void);
void command_update(char c) { //由main调用
	if(c != 0) {
		serial_write(SERIAL1,c); //发往串口
	} else {
		//serial_write(SERIAL1,'<');
	}
	
	if(is_prefix(c) && state == FREE) { //收到指令开头
		state = START;
		prefix = c;
		//printf("prefix = %c\n",prefix);
	} else if(state == START) { //收到空格
		if(c == ' ') {
			state = PARAM;
		} else {
			printf("start error\n");
			state = FREE;
		}
		//printf("blank\n");
	} else if(state == PARAM) { //开始解析参数
		if(is_num(c)) {
			buffer[i] = c;
			i++;
			//printf("p += %c\n",c);
		} else if(c == 0) {
			if(zero_received == 1) {
				buffer[i] = 0;
				sscanf(buffer,"%d",&param);
				i = 0;
				zero_received = 0;
				state = HANDLING;
				printf("Hanling command %c %d\n",prefix,param);
			}
		} else {
			printf("param error\n");
			i = 0;
			state = FREE;
		}
	}
	
	if(state == HANDLING)
		return;
	
	if(c == 0 && state != FREE) { //在非空闲状态下收到0
		if(zero_received == 0) { 
			zero_received = 1; //第一次是警告
		} else {
			printf("double zero reset\n");
			i = 0;
			state = FREE;
			zero_received = 0; //第二次直接重置状态
		}
	} else {
		zero_received = 0; //收到一次非0，继续解析
	}
}

int handle_command(void) { //由定时器调用
	if(state != HANDLING)
		return 0;
	int success = 0;
	float m;
	switch(prefix) {
		case 'f':
			m = param * 0.01;
			success = drive_straight(m,fminf(m/3.0f,1.0f));
			break;
		case 'b':
			m = param * 0.01;
			success = drive_straight(-m,fminf(m/3.0f,1.0f));
			break;
		case 'l':
			success = turn_by_degree(param, 90);
			break;
		case 'r':
			success = turn_by_degree(-param, 90);
			break;
	}
	if(success) {
		state = FREE;
	}
	return 1;
}
